import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class Muurvolger{

	static int distance = 30;
	int difference;
	TachoPilot p;

	int angle = 45;
	SimpleNavigator nav;
	boolean forward;
	int left = 1;
	float side;
	private boolean pause;
	Thread t;
	Thread move;
	Thread w;
	Master master;
	

	class Move implements Runnable {
		public void run(){
			while(true){
				while(!pause){
					// Voorwaarts rijden ("oneindig" lang) indien dit toegelaten wordt
					if(forward)
						p.travel(10000,true);
					// Anders een boog maken in een richting die bepaald wordt door 
					// de kant waar de muur staat, en hoever de muur is.
					else 
						p.steer(left*side*35,angle,true);
				}
			}
		}
	}

	public Muurvolger(TachoPilot p, Master m)
	{
		// Alles initialiseren.
		this.p = p;
		master = m;
		move = new Thread(new Move());
		w = new Thread(new wallFollow());
		move.start();
		w.start();
		forward = true;

	}
	
	public void pause(){
		pause = true;
		p.stop();
	}

	public void restart(){
		pause = false;
	}


	// Hier gebeurt de beslissing of de muur te dicht/ver is, of dat we rechtdoor
	// mogen blijven rijden.
	class wallFollow implements Runnable{
		public void run(){
			while(true){
				try{
					float ultra = master.getDistance();
					float currentDis = (float) Math.cos(angle)*ultra;
					if(currentDis < 0.8*distance){
						side = (float)Math.sqrt((((float) distance)/((float)currentDis)));
						// Als de muur echt te dichtbij komt, sneller wegdraaien.
						if(currentDis < .5*distance){
							side = 1.5f;
						}
						forward = false;
					}
					else if(currentDis > 1.2*distance){
						side =-(float)(Math.sqrt((((float) currentDis)/((float)distance))*0.9));
						// Als de muur "verdwijnt" of een scherpe buitenbocht maakt, sneller
						// meedraaien.
						if(currentDis > 1.9*distance){
							side = -1.3f;
						}
						forward = false;
					}
					else{
						forward = true;
					}
					

				}
				catch(Exception e){
					System.out.println("EXCEPTION" + e.toString());
					try {
						Thread.sleep(2000);
					} catch (InterruptedException e1) {

					}
				}
			}
		}
	}

}
